#Added Mutex
require "Benchmark"
require "thread"

#Added Math module
require "gui_control_lib/math/mathmodule.rb"
include Mathmodule

#Added the Sensor modules
require "gui_control_lib/level1/support/modules/sensormodule.rb"
include Sensormodule

#need to set
#map.autoupdatemap(true)
#map.autologmap(true)
#at the main program


class Occmap
  #public variables
   attr_reader :mapname
   attr_reader :mapwidth
   attr_reader :mapheight
   attr_reader :cellwidth
   attr_reader :cellheight
  
  def initialize(mapname,mapwidth,mapheight,cellwidth,cellheight)
   @mapname=mapname    
   @mapwidth=mapwidth
   @mapheight=mapheight
   @cellwidth=cellwidth
   @cellheight=cellheight
   
   
   @initvalue=0.5
    
   @maxwidth=@mapwidth/2
   @maxheight=@mapheight/2
   
   #make into a two dimension array 
   @Q1map=Array.new(@maxwidth) {Array.new(@maxheight)}
   @Q2map=Array.new(@maxwidth) {Array.new(@maxheight)}
   @Q3map=Array.new(@maxwidth) {Array.new(@maxheight)}
   @Q4map=Array.new(@maxwidth) {Array.new(@maxheight)}
   
   #initiate the values
   @Q1map.each{|member|
     member.map!{ @initvalue }
   }
   @Q2map.each{|member|
     member.map!{ @initvalue }
   }
   @Q3map.each{|member|
     member.map!{ @initvalue }
   }
   @Q4map.each{|member|
     member.map!{ @initvalue }
   }
   #@Q1map=x,y
  
   #self.initsensormodel    

  end
  
  def putmap(x,y,value)
    if(x>(@maxwidth) or y>(@maxheight) or x<-1*(@maxwidth) or y<-1*(@maxheight))
      return false
    end
    if(x>=0 and y>=0)
       @Q1map[x][y]=value
       return true 
    end
     if(x>=0 and y<=0)
       @Q2map[x][y]=value
       return true 
    end
     if(x<=0 and y<=0)
       @Q3map[x][y]=value
       return true 
    end
     if(x<=0 and y>=0)
       @Q4map[x][y]=value
       return true 
    end
    
  end
  
  
  def getmap(x,y)
    return 0.5                 if(x>(@maxwidth) or y>(@maxheight) or x<-1*(@maxwidth) or y<-1*(@maxheight))
    return @Q1map[x][y].to_f   if(x>=0 and y>=0)
    return @Q2map[x][y].to_f   if(x>=0 and y<=0)
    return @Q3map[x][y].to_f   if(x<=0 and y<=0)
    return @Q4map[x][y].to_f   if(x<=0 and y>=0)
  end
  
   def converttocell(x,y)
     cellpos=[]
     cellpos<<(x.to_f/@cellwidth).to_i
     cellpos<<(y.to_f/@cellheight).to_i
     return cellpos
 end
  
end

class Robotmap
  
  #start the mapping sequence
  def initialize(robot)
    
  #debug and testing  
  @debug =1
  @timescalled=0.0
  @avetime=0.0
  @total=0.0
    
  @updateinterval=0.5
  @loginterval=1.5

  @robotobj=robot
  
  
  #robot data 
  @robotsize=0.5 
    
  #sensor infomation
  @lasermaxrange=19.0
 
  #used for the standard sensor model
  @laserrange=20.0
  @laserscale=10.0
  @laserdistsize=@laserrange*@laserscale
  @laserdist=Array.new()
    
  #Thread 
  @mapaccess= Mutex.new  
    
  #flags
  @update=0
  @log=0
  @logobject=1
  
  #autoscans
  @logobjectint=0.8
  
  #data transmission arrays
  @victim=[]
  @minvicdist=20.0
    
  #use ther true robot position
  @usetrue=true
  
  #belief map data   
  mapwidth=600 
  mapheight=600
  cellwidth=0.08
  cellheight=0.08  
  #@initvalue=0.5
  @scanfre=180  
  
  puts "Initializing Belief map "  
  @mapbel=Occmap.new("Belmap",mapwidth,mapheight,cellwidth,cellheight)
       
  end
  


  def autologmap(value)
     if (value==true)
      puts "starting auto update"
      @log=1
      self.autolog
      self.logobject
    else
      @log=0
    end
  end
  
  def autoupdatemap(value)
    if (value==true)
      puts "starting auto update"
      @update=1
      self.updatemap
    else
      @update=0
    end
  end
  
  
  def updatemap
    @m_update_t=Thread.new{
     Thread.current["name"]="map_update"
     while @update==1   
     robot=@robotobj
     robot.recieve#update data on robot
     # @mapaccess.synchronize do     
     self.updatelaser(robot,@mapbel)
     # end
     sleep(@updateinterval)  
     end
     
    }
  end
  
def updatelaser(robot,maptype)
    if(robot.moveflag=="forward")
          #self.ruby_maplaseroverwrite(robot,maptype)
          #self.ruby_maplaserincrement(robot,maptype)
          #self.ruby_maplaserbayes(robot,maptype)
          self.ruby_maplaserbayes_stand(robot,0.02,maptype)
    end
end

def standarddist(wallpos,distri)
 totalscan= @laserscale*@laserrange
 Mathmodule::math_standarddist(wallpos,totalscan,@laserscale,distri) 
end  
  
def initsensormodel()
  @laserdist=self.standarddist(10.0,0.1)
  puts "sensormodel initiated"
end
  
def bayesupdate(oldprob,sensorprob)
 return Mathmodule::math_bayesupdate(oldprob,sensorprob)
end  
  

def logobject
   @m_autoscan_t=Thread.new{
   Thread.current["name"]="map_autoscan"
    while @logobject==1  
      #log the objects
      robot=@robotobj
      self.scanvictim(robot,@mapbel)  if(robot.victsen)#log the map changes to the data    
      self.mapins(robot,@mapbel)
      self.logins(@mapbel)
      sleep(@logobjectint)  
    end
    }
  
end

  
def autolog
    @m_autolog_t=Thread.new{
   Thread.current["name"]="map_logmap"
    while @log==1  
      #@mapaccess.synchronize do
      self.logdata(@mapbel) #log the map changes to the data    
      #end
      sleep(@loginterval)  
    end
    }
  
end

   def scanvictim(robot,maptype)
      unless(robot.victsen=~/NoVictims/i)
      puts "Victim found"
      parts=(robot.victsen.size)/2.0
      puts "#{parts} parts found"
      #find the average position of the parts
      avex=0
      avey=0
      robot.victsen.each_with_index{|member,index|
        if(index%2==1) #location
          temparray=member.split(",")
          avex=avex+temparray[0].to_f
          avey=avey+temparray[1].to_f
        end
      }
      avex=avex/parts
      avey=avey/parts
      vicdist=dist(0,0,avex,avey)
      puts "Average distance=#{vicdist}"
      #puts "Average position: x=#{avex} y=#{avey} "
      vicposx=(vicdist*(Math.cos(robot.ins[5].to_f)))
      vicposy=(vicdist*(Math.sin(robot.ins[5].to_f)))
      puts "Average position: x=#{vicposx} y=#{vicposy} "
      cellpos=maptype.converttocell(vicposx+robot.ins[0].to_f,vicposy+robot.ins[1].to_f)   
      #update map to make victim position  inaccessable
      if(robot.moveflag=="forward") #insert only when moving forward
      2.times{|xoffset|
       2.times{|yoffset|
         maptype.putmap(cellpos[0]+(xoffset-1),cellpos[1]+(yoffset-1),0.90)
       }
      }
      end
      #log the Victim position
      
      #if victim area is empty
      if(@victim.size==0)
           @victim<<cellpos
              puts "#{@victim.size} known"
      end
      
   #if not empty, remember only if it's size is more than @minvicdist meters from every member
    @victim.each_with_index { |member,index|
    if(dist(member[0],member[1],cellpos[0],cellpos[1])<@minvicdist ) #not the same cell
      break
    end
    if(index+1==@victim.size) #able to insert
      if(robot.moveflag=="forward") #insert only when moving forward
        @victim<<cellpos
        puts "#{@victim.size} victims known"
      end
    end
    }

    end
  end
#  def scanvictim(robot)
#      unless(robot.victsen=~/NoVictims/i)
#      puts "Victim found"
#      parts=(robot.victsen.size)/2.0
#      puts "#{parts} parts found"
#      #find the average position of the parts
#      avex=0
#      avey=0
#      robot.victsen.each_with_index{|member,index|
#        if(index%2==1) #location
#          temparray=member.split(",")
#          avex=avex+temparray[0].to_f
#          avey=avey+temparray[1].to_f
#        end
#      }
#      avex=avex/parts
#      avey=avey/parts
#      vicdist=dist(0,0,avex,avey)
#      puts "Average distance=#{vicdist}"
#      #puts "Average position: x=#{avex} y=#{avey} "
#      vicposx=(vicdist*(Math.cos(robot.ins[5].to_f)))
#      vicposy=(vicdist*(Math.sin(robot.ins[5].to_f)))
#      puts "Average position: x=#{vicposx} y=#{vicposy} "
#      cellpos=self.converttocell(vicposx+robot.ins[0].to_f,vicposy+robot.ins[1].to_f)   
#      #update map to make victim position  inaccessable
#      2.times{|xoffset|
#       2.times{|yoffset|
#         self.putmap(cellpos[0]+(xoffset-1),cellpos[1]+(yoffset-1),0.90)
#       }
#      }
#      #log the Victim position
#      
#      #if victim area is empty
#      if(@victim.size==0)
#           @victim<<cellpos
#              puts "#{@victim.size} known"
#      end
#      
#   #if not empty, remember only if it's size is more than @minvicdist meters from every member
#    @victim.each_with_index { |member,index|
#    if(dist(member[0],member[1],cellpos[0],cellpos[1])<@minvicdist) #not the same cell
#      break
#    end
#    if(index+1==@victim.size) #able to insert
#      @victim<<cellpos
#          puts "#{@victim.size} victims known"
#    end
#    }
#
#    end
#  end

  def mapins(robot,maptype)
     cellpos=maptype.converttocell(robot.ins[0],robot.ins[1])
     maptype.putmap(cellpos[0],cellpos[1],0.0)
  end


   
  def logdata(maptype)
   robot=@robotobj
   logbench=Benchmark.measure{  
   #Prepare the data in an array first
   inputstring=""
     maptype.mapwidth.times{|y|
         maptype.mapheight.times{|x|
          inputstring<<(maptype.getmap(x-(maptype.mapwidth/2),y-(maptype.mapheight/2))).to_s<<","      
         }
         inputstring<<("\n")
    }     
      
   #log the map
 open("../../mapdata/occ_#{robot.robotname}_data.dat","w")do|file|
     #put map size 
     file.puts("#{maptype.mapwidth},#{maptype.mapheight}")
     #put data into file
     file.print(inputstring)
   end
  
   
   }
   bench_array=logbench.to_a
   puts "logbench #{(bench_array[1].to_f+bench_array[2].to_f)}" if(@debug==1)
  end
  
 def logins(maptype)
      robot=@robotobj    
      open("../../mapdata/occ_#{robot.robotname}_obj.dat","w")do|file|
      #log the robotic INS
      out2="" 
      out2<<robot.status[0]<<","<<robot.robotname<<",INS,"
      xycell=maptype.converttocell(robot.ins[0].to_f,robot.ins[1].to_f)
      out2<<xycell[0].to_s<<","<<xycell[1].to_s<<",0.0,"<<robot.ins[3]<<","<<robot.ins[4]<<","<<robot.ins[5]
      file.puts(out2)
  
      #log the victims
      @victim.each_with_index { |member,index|
      out2=""
        if(robot)
          out2<<robot.status[0] #Base time on first robot
        else
          out2<<0.00
        end
        out2<<",VICTIM,"<<member[0].to_s<<","<<member[1].to_s
        file.puts(out2)
      }
   end
   
 end
 

end





